#include "parse.h"
#include "stdbool.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "gcode.h"



/***************************************************
*	Replace CR/LF to EOL char.
****************************************************/
char* str_trim(char* str)
{
    char ch, * p = str;
    while ((ch = *p) != '\0')
    {
        if (ch == '\n' || ch == '\r')
        {
            *p = '\0';
            break;
        }
        p++;
    }
    return str;
}

double strtod_M(const char* str, char** endptr)
{
    double number = 0.0;
    double div = 0.0;
    bool negative = false;
    bool plus = false;
    bool skip = true;
    char c;
    while ((c = *str) != 0)
    {
        if (c == '+')
        {
            if (skip && !plus)
            {
                plus = true;
                skip = false;
            }
            else
            {
                break;
            }
        }
        else if (skip && !negative && c == '-')
        {
            if (skip && !negative)
            {
                negative = true;
                skip = false;
            }
            else
            {
                break;
            }
        }
        else if (c == '.')
        {
            if (div == 0.0)
            {
                div = 1.0;
            }
            else
            {
                break;
            }
        }
        else if (c >= '0' && c <= '9')
        {
            skip = false;
            if (div == 0.0)
            {
                number = number * 10.0 + (double)(c - '0');
            }
            else
            {
                div *= 10.0;
                number += ((double)(c - '0') / div);
            }
        }
        else if (!skip)
        {
            break;
        }
        str++;
    }

    if (negative)
    {
        number = -number;
    }
    if (endptr != NULL)
    {
        *endptr = (char*)str;
    }
    return number;
}


//解析Gcode里面跟在命令字后面的double
int read_double(const char* line, int* char_counter, double* double_ptr)
{
    const char* start = line + *char_counter;
    char* end;

    *double_ptr = strtod_M(start, &end);
    if (end == start)
    {
        //        gc.status_code = GCSTATUS_BAD_NUMBER_FORMAT;
        return false;
    }
    *char_counter = (int)(end - line);
    return true;
}

//解析一行Gcode里面的下一个参数
int next_statement(char* letter, double* double_ptr, const char* line, int* char_counter)
{
    while (line[*char_counter] == ' ') (*char_counter)++;

    if (line[*char_counter] == 0 ||
        line[*char_counter] == ';' ||
        line[*char_counter] == '\n' ||
        line[*char_counter] == '\r')
    {
        return false;
    }
    *letter = line[*char_counter];                  //letter是代码的第一个有效数据
    if ((*letter < 'A') || (*letter > 'Z'))         //第一个有效代码不在A,Z之间，则为控制代码
    {
        gc.status_code = GCSTATUS_EXPECTED_COMMAND_LETTER;
        return false;
    }
    (*char_counter)++;
    return read_double(line, char_counter, double_ptr);
}
